/*=========================================================================
UberSim Source Code Release
-------------------------------------------------------------------------
Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
                   James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
                   Erick Tryzelaar {erickt}@andrew.cmu.edu
School of Computer Science, Carnegie Mellon University
-------------------------------------------------------------------------
This software is distributed under the GNU General Public License,
version 2.  If you do not have a copy of this licence, visit
www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
in the hope that it will be useful, but WITHOUT ANY WARRANTY,
including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-------------------------------------------------------------------------*/

#include "mmgr.h"

#include "Island.h"
#include "Joint.h"
#include "RigidBody.h"

Joint::~Joint ()
{
}

RigidBody* Joint::getRigidBody1 ()
{
	return rigidBody1;
}

RigidBody* Joint::getRigidBody2 ()
{
	return rigidBody2;
}

Joint::Joint (RigidBody* rigidBody1, RigidBody* rigidBody2)
{
	this->rigidBody1 = rigidBody1;
	this->rigidBody2 = rigidBody2;

	Island* island1 = rigidBody1->getIsland ();
	Island* island2 = rigidBody2->getIsland ();

	if (island1 && island2)
	{
		if (island1->size () < island2->size ())
		{
			for (unsigned int i = 0; i < island1->size (); i++)
			{
				RigidBody* rigidBody = island1->getRigidBody (i);

				if (rigidBody)
				{
					island2->addRigidBody (rigidBody);
				}
			}

			delete island1;
		}
		else
		{
			for (unsigned int i = 0; i < island2->size (); i++)
			{
				RigidBody* rigidBody = island2->getRigidBody (i);

				if (rigidBody)
				{
					island1->addRigidBody (rigidBody);
				}
			}

			delete island2;
		}
	}
	else if (island1)
	{
		island1->addRigidBody (rigidBody2);
	}
	else if (island2)
	{
		island2->addRigidBody (rigidBody1);
	}
	else
	{
		Island* island = new Island ();
	
		island->addRigidBody (rigidBody1);
		island->addRigidBody (rigidBody2);
	}
}
